#include "DataSerialization.h"

uint64_t pack754(double f, unsigned bits, unsigned expbits) {
  double fnorm;
  int shift;
  long long sign, exp, significand;
  unsigned significandbits = bits - expbits - 1;  // -1 for sign bit

  if (f == 0.0) return 0;  // get this special case out of the way

  // check sign and begin normalization
  if (f < 0) {
    sign = 1;
    fnorm = -f;
  } else {
    sign = 0;
    fnorm = f;
  }

  // get the normalized form of f and track the exponent
  shift = 0;
  while (fnorm >= 2.0) {
    fnorm /= 2.0;
    shift++;
  }
  while (fnorm < 1.0) {
    fnorm *= 2.0;
    shift--;
  }
  fnorm = fnorm - 1.0;

  // calculate the binary form (non-float) of the significand data
  significand = fnorm * ((1LL << significandbits) + 0.5f);

  // get the biased exponent
  exp = shift + ((1 << (expbits - 1)) - 1);  // shift + bias

  // return the final answer
  return (sign << (bits - 1)) | (exp << (bits - expbits - 1)) | significand;
}  // pack754

double unpack754(uint64_t i, unsigned bits, unsigned expbits) {
  double result;
  int64_t shift;
  unsigned bias;
  unsigned significandbits = bits - expbits - 1;  // -1 for sign bit

  if (i == 0) return 0.0;

  // pull the significand
  result = (i & ((1LL << significandbits) - 1));  // mask
  result /= (1LL << significandbits);             // convert back to float
  result += 1.0f;                                 // add the one back on

  // deal with the exponent
  bias = (1 << (expbits - 1)) - 1;
  shift = ((i >> significandbits) & ((1LL << expbits) - 1)) - bias;
  while (shift > 0) {
    result *= 2.0;
    shift--;
  }
  while (shift < 0) {
    result /= 2.0;
    shift++;
  }

  // sign it
  result *= (i >> (bits - 1)) & 1 ? -1.0 : 1.0;

  return result;
}  // unpack754

void packi16(uint8_t *buf, uint16_t i) {
  *buf++ = i >> 8;
  *buf++ = i;
}  // packi16

void packi32(uint8_t *buf, uint32_t i) {
  *buf++ = i >> 24;
  *buf++ = i >> 16;
  *buf++ = i >> 8;
  *buf++ = i;
}  // packi32

void packi64(uint8_t *buf, uint64_t i) {
  *buf++ = i >> 56;
  *buf++ = i >> 48;
  *buf++ = i >> 40;
  *buf++ = i >> 32;
  *buf++ = i >> 24;
  *buf++ = i >> 16;
  *buf++ = i >> 8;
  *buf++ = i;
}  // packi64

int16_t unpacki16(const uint8_t *buf) {
  uint16_t i2 = ((uint16_t)buf[0] << 8) | buf[1];
  int16_t i;

  // change unsigned numbers to signed
  if (i2 <= 0x7fffu) {
    i = i2;
  } else {
    i = -1 - (uint16_t)(0xffffu - i2);
  }

  return i;
}  // unpacki16

uint16_t unpacku16(const uint8_t *buf) {
  return ((uint16_t)buf[0] << 8) | buf[1];
}  // unpacku16

int32_t unpacki32(const uint8_t *buf) {
  uint32_t i2 = ((uint32_t)buf[0] << 24) | ((uint32_t)buf[1] << 16) |
                ((uint32_t)buf[2] << 8) | buf[3];
  int32_t i;

  // change unsigned numbers to signed
  if (i2 <= 0x7fffffffu) {
    i = i2;
  } else {
    i = -1 - (int32_t)(0xffffffffu - i2);
  }

  return i;
}  // unpacki32

uint32_t unpacku32(const uint8_t *buf) {
  return ((uint32_t)buf[0] << 24) | ((uint32_t)buf[1] << 16) |
         ((uint32_t)buf[2] << 8) | buf[3];
}  // unpacku32

int64_t unpacki64(const uint8_t *buf) {
  uint64_t i2 = ((uint64_t)buf[0] << 56) | ((uint64_t)buf[1] << 48) |
                ((uint64_t)buf[2] << 40) | ((uint64_t)buf[3] << 32) |
                ((uint64_t)buf[4] << 24) | ((uint64_t)buf[5] << 16) |
                ((uint64_t)buf[6] << 8) | buf[7];
  int64_t i;

  // change unsigned numbers to signed
  if (i2 <= 0x7fffffffffffffffu) {
    i = i2;
  } else {
    i = -1 - (int64_t)(0xffffffffffffffffu - i2);
  }

  return i;
}  // unpacki64

uint64_t unpacku64(const uint8_t *buf) {
  return ((uint64_t)buf[0] << 56) | ((uint64_t)buf[1] << 48) |
         ((uint64_t)buf[2] << 40) | ((uint64_t)buf[3] << 32) |
         ((uint64_t)buf[4] << 24) | ((uint64_t)buf[5] << 16) |
         ((uint64_t)buf[6] << 8) | buf[7];
}  // unpacku64

int16_t pack(uint8_t *buf, const char *format, ...) {
  va_list ap;

  int8_t c;  // 8-bit
  uint8_t C;

  int16_t h;  // 16-bit
  uint16_t H;

  int32_t l;  // 32-bit
  uint32_t L;

  int64_t q;  // 64-bit
  uint64_t Q;

  float d;  // float
  double g;

  uint64_t fhold;

  char *s;  // strings
  uint32_t len;

  uint32_t size = 0;

  va_start(ap, format);

  for (; *format != '\0'; format++) {
    switch (*format) {
      case 'c':  // 8-bit
        size += 1;
        c = (int8_t)va_arg(ap, int32_t);  // promoted
        *buf++ = c;
        break;

      case 'C':  // 8-bit unsigned
        size += 1;
        C = (uint8_t)va_arg(ap, uint32_t);  // promoted
        *buf++ = C;
        break;

      case 'h':  // 16-bit
        size += 2;
        h = (int16_t)va_arg(ap, int32_t);
        packi16(buf, h);
        buf += 2;
        break;

      case 'H':  // 16-bit unsigned
        size += 2;
        H = (uint16_t)va_arg(ap, uint32_t);
        packi16(buf, H);
        buf += 2;
        break;

      case 'l':  // 32-bit
        size += 4;
        l = va_arg(ap, int32_t);
        packi32(buf, l);
        buf += 4;
        break;

      case 'L':  // 32-bit unsigned
        size += 4;
        L = va_arg(ap, uint32_t);
        packi32(buf, L);
        buf += 4;
        break;

      case 'q':  // 64-bit
        size += 8;
        q = va_arg(ap, int64_t);
        packi64(buf, q);
        buf += 8;
        break;

      case 'Q':  // 64-bit unsigned
        size += 8;
        Q = va_arg(ap, uint64_t);
        packi64(buf, Q);
        buf += 8;
        break;

      case 'd':  // float-32
        size += 4;
        d = va_arg(ap, double);
        fhold = pack754(d, 32, 8);  // convert to IEEE 754
        packi32(buf, fhold);
        buf += 4;
        break;

      case 'g':  // float-64
        size += 8;
        g = va_arg(ap, double);
        fhold = pack754(g, 64, 11);  // convert to IEEE 754
        packi64(buf, fhold);
        buf += 8;
        break;

      case 's':  // string
        s = va_arg(ap, char *);
        len = strlen(s);
        size += len + 2;
        packi16(buf, len);
        buf += 2;
        memcpy(buf, s, len);
        buf += len;
        break;
    }
  }

  va_end(ap);

  return size;
}  // pack

void unpack(const uint8_t *buf, const char *format, ...) {
  va_list ap;

  int8_t *c;  // 8-bit
  uint8_t *C;

  int16_t *h;  // 16-bit
  uint16_t *H;

  int32_t *l;  // 32-bit
  uint32_t *L;

  int64_t *q;  // 64-bit
  uint64_t *Q;

  float *d;
  double *g;
  uint64_t fhold;

  char *s;
  uint32_t len, maxstrlen = 0, count;

  va_start(ap, format);

  for (; *format != '\0'; format++) {
    switch (*format) {
      case 'c':  // 8-bit
        c = va_arg(ap, int8_t *);
        if (*buf <= 0x7f) {
          *c = *buf;
        }  // re-sign
        else {
          *c = -1 - (uint8_t)(0xffu - *buf);
        }
        buf++;
        break;

      case 'C':  // 8-bit unsigned
        C = va_arg(ap, uint8_t *);
        *C = *buf++;
        break;

      case 'h':  // 16-bit
        h = va_arg(ap, int16_t *);
        *h = unpacki16(buf);
        buf += 2;
        break;

      case 'H':  // 16-bit unsigned
        H = va_arg(ap, uint16_t *);
        *H = unpacku16(buf);
        buf += 2;
        break;

      case 'l':  // 32-bit
        l = va_arg(ap, int32_t *);
        *l = unpacki32(buf);
        buf += 4;
        break;

      case 'L':  // 32-bit unsigned
        L = va_arg(ap, uint32_t *);
        *L = unpacku32(buf);
        buf += 4;
        break;

      case 'q':  // 64-bit
        q = va_arg(ap, int64_t *);
        *q = unpacki64(buf);
        buf += 8;
        break;

      case 'Q':  // 64-bit unsigned
        Q = va_arg(ap, uint64_t *);
        *Q = unpacku64(buf);
        buf += 8;
        break;

      case 'd':  // float-32
        d = va_arg(ap, float *);
        fhold = unpacku32(buf);
        *d = unpack754(fhold, 32, 8);
        buf += 4;
        break;

      case 'g':  // float-64
        g = va_arg(ap, double *);
        fhold = unpacku64(buf);
        *g = unpack754(fhold, 64, 11);
        buf += 8;
        break;

      case 's':  // string
        s = va_arg(ap, char *);
        len = unpacku16(buf);
        buf += 2;
        if (maxstrlen > 0 && len > maxstrlen)
          count = maxstrlen - 1;
        else
          count = len;
        memcpy(s, buf, count);
        s[count] = '\0';
        buf += len;
        break;

      default:
        if (isdigit(*format)) {  // track max str len
          maxstrlen = maxstrlen * 10 + (*format - '0');
        }
    }

    if (!isdigit(*format)) maxstrlen = 0;
  }

  va_end(ap);
}  // unpack

uint16_t CRC16(const uint8_t *nData, uint16_t wLength) {
  static const uint16_t wCRCTable[] = {
      0X0000, 0XC0C1, 0XC181, 0X0140, 0XC301, 0X03C0, 0X0280, 0XC241, 0XC601,
      0X06C0, 0X0780, 0XC741, 0X0500, 0XC5C1, 0XC481, 0X0440, 0XCC01, 0X0CC0,
      0X0D80, 0XCD41, 0X0F00, 0XCFC1, 0XCE81, 0X0E40, 0X0A00, 0XCAC1, 0XCB81,
      0X0B40, 0XC901, 0X09C0, 0X0880, 0XC841, 0XD801, 0X18C0, 0X1980, 0XD941,
      0X1B00, 0XDBC1, 0XDA81, 0X1A40, 0X1E00, 0XDEC1, 0XDF81, 0X1F40, 0XDD01,
      0X1DC0, 0X1C80, 0XDC41, 0X1400, 0XD4C1, 0XD581, 0X1540, 0XD701, 0X17C0,
      0X1680, 0XD641, 0XD201, 0X12C0, 0X1380, 0XD341, 0X1100, 0XD1C1, 0XD081,
      0X1040, 0XF001, 0X30C0, 0X3180, 0XF141, 0X3300, 0XF3C1, 0XF281, 0X3240,
      0X3600, 0XF6C1, 0XF781, 0X3740, 0XF501, 0X35C0, 0X3480, 0XF441, 0X3C00,
      0XFCC1, 0XFD81, 0X3D40, 0XFF01, 0X3FC0, 0X3E80, 0XFE41, 0XFA01, 0X3AC0,
      0X3B80, 0XFB41, 0X3900, 0XF9C1, 0XF881, 0X3840, 0X2800, 0XE8C1, 0XE981,
      0X2940, 0XEB01, 0X2BC0, 0X2A80, 0XEA41, 0XEE01, 0X2EC0, 0X2F80, 0XEF41,
      0X2D00, 0XEDC1, 0XEC81, 0X2C40, 0XE401, 0X24C0, 0X2580, 0XE541, 0X2700,
      0XE7C1, 0XE681, 0X2640, 0X2200, 0XE2C1, 0XE381, 0X2340, 0XE101, 0X21C0,
      0X2080, 0XE041, 0XA001, 0X60C0, 0X6180, 0XA141, 0X6300, 0XA3C1, 0XA281,
      0X6240, 0X6600, 0XA6C1, 0XA781, 0X6740, 0XA501, 0X65C0, 0X6480, 0XA441,
      0X6C00, 0XACC1, 0XAD81, 0X6D40, 0XAF01, 0X6FC0, 0X6E80, 0XAE41, 0XAA01,
      0X6AC0, 0X6B80, 0XAB41, 0X6900, 0XA9C1, 0XA881, 0X6840, 0X7800, 0XB8C1,
      0XB981, 0X7940, 0XBB01, 0X7BC0, 0X7A80, 0XBA41, 0XBE01, 0X7EC0, 0X7F80,
      0XBF41, 0X7D00, 0XBDC1, 0XBC81, 0X7C40, 0XB401, 0X74C0, 0X7580, 0XB541,
      0X7700, 0XB7C1, 0XB681, 0X7640, 0X7200, 0XB2C1, 0XB381, 0X7340, 0XB101,
      0X71C0, 0X7080, 0XB041, 0X5000, 0X90C1, 0X9181, 0X5140, 0X9301, 0X53C0,
      0X5280, 0X9241, 0X9601, 0X56C0, 0X5780, 0X9741, 0X5500, 0X95C1, 0X9481,
      0X5440, 0X9C01, 0X5CC0, 0X5D80, 0X9D41, 0X5F00, 0X9FC1, 0X9E81, 0X5E40,
      0X5A00, 0X9AC1, 0X9B81, 0X5B40, 0X9901, 0X59C0, 0X5880, 0X9841, 0X8801,
      0X48C0, 0X4980, 0X8941, 0X4B00, 0X8BC1, 0X8A81, 0X4A40, 0X4E00, 0X8EC1,
      0X8F81, 0X4F40, 0X8D01, 0X4DC0, 0X4C80, 0X8C41, 0X4400, 0X84C1, 0X8581,
      0X4540, 0X8701, 0X47C0, 0X4680, 0X8641, 0X8201, 0X42C0, 0X4380, 0X8341,
      0X4100, 0X81C1, 0X8081, 0X4040};

  uint8_t nTemp;
  uint16_t wCRCWord = 0xFFFF;

  while (wLength--) {
    nTemp = *nData++ ^ wCRCWord;
    wCRCWord >>= 8;
    wCRCWord ^= wCRCTable[nTemp];
  }
  return wCRCWord;
}  // CRC16
